﻿#include <PhysicsWorld.h>
#include <UserDefinedSphereSphereCollisionAlgorithm.h>
#include <SDFCollisionAlgorithm.h>
#include <PhysicsWorldFunctions.h>
#include <SDFDebugDrawer.h>
#include <SDFManager.h>
//#include <OpenTissueFunction.h>
PhysicsWorld* PhysicsWorld::_instance = NULL;



PhysicsWorld::~PhysicsWorld()
{

}

PhysicsWorld* PhysicsWorld::instance()
{
	if (_instance == NULL)
	{
		_instance = new PhysicsWorld();
	}
	return _instance;

}

PhysicsWorld::PhysicsWorld()
{
    m_userObjectPointer = NULL;
	initPhysicsWorld();

    m_socketSender = new QUdpSocket(0);
}

btDiscreteDynamicsWorld* PhysicsWorld::getDynamicsWorld()
{
	return _discrete_dynamics_world;
}

osgbCollision::GLDebugDrawer* PhysicsWorld::getDebugDraw()
{
	return _debug_draw;
}


void PhysicsWorld::addRigidBody( btRigidBody* rigid_body )
{
    if(rigid_body != NULL)
    {
        _discrete_dynamics_world->addRigidBody(rigid_body);
    }

}

void PhysicsWorld::removeRigidBody(btRigidBody *rigid_body)
{
    _discrete_dynamics_world->removeRigidBody(rigid_body);
}



btRigidBody* PhysicsWorld::getRigidBody(int int_id)
{
	const btCollisionObjectArray& object_array = _discrete_dynamics_world->getCollisionObjectArray();
	
	btRigidBody* geted_rigid_body = NULL;

	int i;
	for(i = 0; i < object_array.size(); i++)
	{
		btCollisionObject* object_ptr = object_array.at(i);
		int object_index = object_ptr->getUserIndex();

		if (object_index == int_id)
		{
			geted_rigid_body = dynamic_cast<btRigidBody*>(object_ptr);
			break;
		}
	}
    return geted_rigid_body;
}

const btCollisionObjectArray *PhysicsWorld::getCollisionObjArray()
{
    const btCollisionObjectArray& object_array = _discrete_dynamics_world->getCollisionObjectArray();
    return &object_array;
}

void PhysicsWorld::frame()
{

}

void PhysicsWorld::frame(double current_sim_time, double previous_sim_time)
{
	//std::cout<<"bulelt"<<std::endl;
	if (_enable_debug_draw == true)
	{
		_debug_draw->BeginDraw();
	}

	if (_enable_dynamic == true)
	{
		_discrete_dynamics_world->stepSimulation( current_sim_time - previous_sim_time );
		//std::cout<<"动力学"<<std::endl;
	}
	else if (_enable_real_time_collisioin_detection == true)
	{
		_discrete_dynamics_world->performDiscreteCollisionDetection();
		//std::cout<<"碰撞检测"<<std::endl;
	}
	
	if (_enable_debug_draw == true)
	{
		_discrete_dynamics_world->debugDrawWorld();
		_debug_draw->EndDraw();
	}
}

void PhysicsWorld::setEnableDebugDraw(bool enable_or_disable)
{
	_enable_debug_draw = enable_or_disable;
}

void PhysicsWorld::setEnableDynamic(bool enable_dynamic)
{
	_enable_dynamic = enable_dynamic;
}

void PhysicsWorld::setEnableRealTimeCollisioinDetection(bool enable_real_time_collisioin_detection)
{
	_enable_real_time_collisioin_detection = enable_real_time_collisioin_detection;
}


bool PhysicsWorld::getEnableDebugDraw()
{
	return _enable_debug_draw;
}
bool PhysicsWorld::getEnableRealTimeCollisioinDetection()
{
    return _enable_real_time_collisioin_detection;
}

void PhysicsWorld::performCollisionOnce()
{
    //PhysicsWorldFunctions::writeRigidBodyInfoToXML();
    //PhysicsWorldFunctions::writeZipOfRigidBody();
    //PhysicsWorldFunctions::sendZipTOSocket();
    SDFManager::instance()->clearSDFContacts();

    _discrete_dynamics_world->performDiscreteCollisionDetection();

    //SDFManager::instance()->printContacts();
    SDFManager::instance()->updateContactDebugDrawerNode();

    std::cout<<"PhysicsWorld::performCollisionOnce"<<SDFManager::instance()->getSDFContactsCount()<<std::endl;
}

void *PhysicsWorld::getUserPointer()
{
    return m_userObjectPointer;
}

void PhysicsWorld::setUserPointer(void *userPointer)
{
    m_userObjectPointer = userPointer;
}

QUdpSocket *PhysicsWorld::getSenderSocket()
{
    return m_socketSender;
}

void PhysicsWorld::initPhysicsWorld()
{
	
	_collision_configuration = new btDefaultCollisionConfiguration;
	//_collision_dispatcher = std::make_shared<btCollisionDispatcher>( _collision_configuration );
	_collision_dispatcher = new btCollisionDispatcher(_collision_configuration);
    //try userDefinedSphereSphereCollisioin
    _collision_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new UserDefinedSphereSphereCollisionAlgorithm::CreateFunc);
    _collision_dispatcher->registerCollisionCreateFunc(TRIANGLE_MESH_SHAPE_PROXYTYPE,TRIANGLE_MESH_SHAPE_PROXYTYPE,new SDFCollisionAlgorithm::CreateFunc);

    //_constraint_solver = std::make_shared< btSequentialImpulseConstraintSolver>();
	_constraint_solver = new btSequentialImpulseConstraintSolver;

	btVector3 worldAabbMin( -10000, -10000, -10000 );
	btVector3 worldAabbMax( 10000, 10000, 10000 );
	//_broad_phase_interface = std::make_shared<btAxisSweep3>( worldAabbMin, worldAabbMax, 1000 );
	_broad_phase_interface = new btAxisSweep3(worldAabbMin, worldAabbMax, 1000 );

	//_discrete_dynamics_world = std::make_shared<btDiscreteDynamicsWorld>( _collision_dispatcher, _broad_phase_interface, _constraint_solver, _collision_configuration );
	_discrete_dynamics_world = new btDiscreteDynamicsWorld( _collision_dispatcher, _broad_phase_interface, _constraint_solver, _collision_configuration);
	_discrete_dynamics_world->setGravity( btVector3( 0, 0, -9.81 ) );

	//_debug_draw = std::make_shared<osgbCollision::GLDebugDrawer>();
    _debug_draw = new SDFDebugDrawer();
    //_debug_draw->setDebugMode( ~btIDebugDraw::DBG_DrawText );
    _debug_draw->setDebugMode(btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawContactPoints
                              | btIDebugDraw::DBG_DrawWireframe );
	_discrete_dynamics_world->setDebugDrawer( _debug_draw );

	_enable_debug_draw = false;
	_enable_dynamic = true;
	_enable_real_time_collisioin_detection = true;

    //OpenTissueFunction::startThread();

}
